#include <iostream>
#include <ros/ros.h>
#include <Eigen/Eigen>
#include "nav_msgs/Odometry.h"
#include "quadrotor_msgs/PositionCommand.h"

int main (int argc, char** argv) 
{   
    ros::init(argc, argv, "restart");
    ros::NodeHandle nh("~");

	ros::Rate looprate(30);
	ros::Publisher restart[4];
	restart[0]=nh.advertise<quadrotor_msgs::PositionCommand>("/drone_0_planning/pos_cmd", 1, true);
	restart[1]=nh.advertise<quadrotor_msgs::PositionCommand>("/drone_1_planning/pos_cmd", 1, true);
	restart[2]=nh.advertise<quadrotor_msgs::PositionCommand>("/drone_2_planning/pos_cmd", 1, true);
	restart[3]=nh.advertise<quadrotor_msgs::PositionCommand>("/drone_3_planning/pos_cmd", 1, true);

	Eigen::Matrix<double,4,3> start_pos;
	start_pos<<   0.0  ,  0  ,  0.1,
				-21.3 ,  0  ,   0.1,
				-20.0 ,  -0.8  , 0.1,
				-20.0,  0.8,    0.1;

    nh.getParam("agent_0_start_x",start_pos(0,0));
    nh.getParam("agent_0_start_y",start_pos(0,1));
    nh.getParam("agent_0_start_z",start_pos(0,2));

    nh.getParam("agent_1_start_x",start_pos(1,0));
    nh.getParam("agent_1_start_y",start_pos(1,1));
    nh.getParam("agent_1_start_z",start_pos(1,2));

    nh.getParam("agent_2_start_x",start_pos(2,0));
    nh.getParam("agent_2_start_y",start_pos(2,1));
    nh.getParam("agent_2_start_z",start_pos(2,2));

    nh.getParam("agent_3_start_x",start_pos(3,0));
    nh.getParam("agent_3_start_y",start_pos(3,1));
    nh.getParam("agent_3_start_z",start_pos(3,2));

	Eigen::Vector3d start_centre;
    nh.getParam("agent_centre_start_x",start_centre(0));
    nh.getParam("agent_centre_start_y",start_centre(1));
    nh.getParam("agent_centre_start_z",start_centre(2));



	start_pos.row(0)+=start_centre.transpose();
	start_pos.row(1)+=start_centre.transpose();
	start_pos.row(2)+=start_centre.transpose();
	start_pos.row(3)+=start_centre.transpose();


	ros::Time start_time=ros::Time::now();
	while (1)
	{

		for(int id=0;id<4;id++)
		{
			quadrotor_msgs::PositionCommand setpose;
			setpose.header.stamp=ros::Time::now();
			setpose.position.x=start_pos(id,0);
			setpose.position.y=start_pos(id,1);
			setpose.position.z=start_pos(id,2);
			restart[id].publish(setpose);
		}


		if((ros::Time::now()-start_time).toSec()>0.8)
		{
			exit(0);
		}
		looprate.sleep();


	}
	
	


}